/**
 * @file:          TrapezoidCurve.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2024.12.09
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2024.12.09,12:04:29
 */

/* Include Files **************************************************************/
#include "TrapezoidCurve.h"
#include "base_math.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void TrapezoidCurve_Init(struct TrapezoidCurve *self, float init_pos)
{
    self->UpdateParam = TrapezoidCurve_UpdateParam;

    self->in_target_pos = init_pos;
    self->target_pos_last = init_pos;
    self->origin_pos = init_pos;
    self->in_fdb_pos = init_pos;
    self->out_cmd_pos = init_pos;
    self->in_target_speed = 0.0F;
    self->target_speed_last = 0.0F;
    self->target_speed_old = 0.0F;

    self->T_count = 0;
    self->T_count_max = 0;

    self->UpdateParam(self);
}

void TrapezoidCurve_UpdateParam(struct TrapezoidCurve *self)
{
    switch (self->param_plan_mode)
    {
    case TrapezoidCurvePlanMode_Pos:
        self->Run = TrapezoidCurve_Run_PosPlan;
        break;
    case TrapezoidCurvePlanMode_Spd:
        self->Run = TrapezoidCurve_Run_SpeedPlan;
        break;
    case TrapezoidCurvePlanMode_PosDoublePlan:
        self->Run = TrapezoidCurve_Run_PosDoublePlan;
        break;
    default:
        self->Run = TrapezoidCurve_Run_PosPlan;
        break;
    }
}

float TrapezoidCurve_Run_PosPlan(struct TrapezoidCurve *self)
{
    if (self->param_auto_plan == TRUE)
    {
        if (abs_bm(self->in_target_pos - self->in_fdb_pos) > 1.0e-1F)
        {
            self->in_start = 1;
            self->target_pos_old = self->target_pos;
            self->target_pos = self->in_target_pos;
        }
        self->target_pos_last = self->in_target_pos;
    }

    /* 延迟启动 */
    if (self->in_start == 1 && self->running_pre == 0)
    {
        self->count = 0;
        self->running_pre = 1;

        self->origin_pos = self->in_fdb_pos;
        self->out_cmd_pos = self->origin_pos;
        self->out_cmd_spd = 0.0F;
        self->out_cmd_acc = 0.0F;
    }
    else if (self->running_pre == 1)
    {
        if (self->count < self->param_start_delay_count)
        {
            self->count++;
        }
        else
        {
            self->start = 1;
        }
    }

    /* 轨迹规划 */
    if (self->start == 1 && self->running == 0)
    {
        // self->start = 0;
        self->running = 1;

        /* plan */
        self->displacement = self->target_pos - self->origin_pos;
        self->dir = sign_bm(self->displacement);

        self->a1 = self->dir * self->param_acc_rad_s2;
        self->a2 = 0;
        self->a3 = -self->dir * self->param_acc_rad_s2;

        /* judge mode */
        self->Tk1 = self->param_max_speed_rad_s / self->param_acc_rad_s2;
        self->sk1 = 0.5 * self->param_acc_rad_s2 * self->Tk1 * self->Tk1;

        if (abs_bm(self->displacement) < 2 * self->sk1)
        {
            self->traj_type = TrapezoidCurveTrajMode_ConstantVelocityNoExist;
        }
        else
        {
            self->traj_type = TrapezoidCurveTrajMode_ConstantVelocityExist;
        }

        switch (self->traj_type)
        {
        case TrapezoidCurveTrajMode_ConstantVelocityNoExist: /* 没有匀速段 */
            self->T1 = bm_sqrt_carmack(abs_bm(self->displacement) / self->param_acc_rad_s2);
            self->T3 = self->T1;

            self->v1 = self->a1 * self->T1;
            self->v2 = self->v1;
            self->v3 = self->v2 + self->a3 * self->T3;
            self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            self->s2 = self->displacement - 2 * self->s1;
            self->s3 = self->s1;

            self->T2 = 0.0F;
            break;
        case TrapezoidCurveTrajMode_ConstantVelocityExist: /* 有匀速段 */
            self->T1 = self->Tk1;
            self->T3 = self->T1;

            self->v1 = self->a1 * self->T1;
            self->v2 = self->v1;
            self->v3 = self->v2 + self->a3 * self->T3;
            self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            self->s2 = self->displacement - 2 * self->s1;
            self->s3 = self->s1;

            self->T2 = self->s2 / self->v1;
            break;
        }
        self->t1 = self->T1;
        self->t2 = self->T1 + self->T2;
        self->t3 = self->T1 + self->T2 + self->T3;

        /* init */
        self->T_count = 0;
        self->t = 0.0F;
        self->T_count_max = (uint32_t)(self->t3 / self->Ts);
    }
    else if (self->running == 1)
    {
        self->t = self->T_count * self->Ts;

        if (self->t < self->t1)
        {
            self->tsec1 = self->t;
            self->out_cmd_pos = self->origin_pos +
                                0.5 * self->a1 * self->tsec1 * self->tsec1;
            self->out_cmd_spd = self->a1 * self->tsec1;
            self->out_cmd_acc = self->a1;
        }
        // else if (self->t <= 3600.0F)
        else if (self->t < self->t2)
        {
            self->tsec2 = self->t - self->t1;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 +
                                self->tsec2 * self->v1;
            self->out_cmd_spd = self->v1;
            self->out_cmd_acc = 0.0F;
        }
        else if (self->t < self->t3)
        {
            self->tsec3 = self->t - self->t2;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 + self->s2 +
                                self->v2 * self->tsec3 +
                                0.5 * self->a3 * self->tsec3 * self->tsec3;
            self->out_cmd_spd = self->v2 * self->t1 + self->a3 * self->tsec3;
            self->out_cmd_acc = self->a3;
        }
        else
        {
            self->start = 0;
            self->running = 0;
            self->finished = 1;
            self->in_target_pos = self->in_fdb_pos;

            self->in_start = 0;
            self->running_pre = 0;
        }

        /* 停止 */
        if (self->stop == 1)
        {
            self->stop = 0;
            self->start = 0;
            self->running = 0;
            self->finished = 0;

            self->in_start = 0;
            self->running_pre = 0;

            self->origin_pos = self->in_fdb_pos;

            self->in_target_pos = self->in_fdb_pos;
            self->target_pos_last = 0.0F;
            self->target_pos_old = 0.0F;
            self->target_pos = 0.0F;
        }

        self->T_count++;
    }

    return self->out_cmd_pos;
}

float TrapezoidCurve_Run_SpeedPlan(struct TrapezoidCurve *self)
{
    if (self->param_auto_plan == TRUE)
    {
        if (abs_bm(self->in_target_speed - self->target_speed_last) > 1.0F)
        {
            self->in_start = 1;
            self->target_speed_old = self->target_speed;
            self->target_speed = self->in_target_speed;
        }
        self->target_speed_last = self->in_target_speed;
    }

    /* 延迟启动 */
    if (self->in_start == 1 && self->running_pre == 0)
    {
        self->count = 0;
        self->running_pre = 1;
    }
    else if (self->running_pre == 1)
    {
        if (self->count < self->param_start_delay_count)
        {
            self->count++;
        }
        else
        {
            self->start = 1;
        }
    }

    /* 轨迹规划 */
    if (self->start == 1 && self->running == 0)
    {
        // self->start = 0;
        self->running = 1;

        /* plan */
        self->t = self->T_count * self->Ts;
        self->tsec2 = self->t - self->t1;
        self->out_cmd_pos = self->origin_pos +
                            self->s1 +
                            self->v1 * self->tsec2;
        self->out_cmd_spd = self->v1;
        self->out_cmd_acc = 0.0F;

        self->origin_pos = self->out_cmd_pos;
        self->origin_speed = self->target_speed_old;
        // self->displacement = self->in_target_pos - self->origin_pos;
        self->dir = sign_bm(self->target_speed - self->origin_speed);

        self->a1 = self->dir * self->param_acc_rad_s2;
        self->a2 = 0;
        self->a3 = 0;

        self->T1 = abs_bm(self->target_speed - self->origin_speed) / self->param_acc_rad_s2;
        self->t1 = self->T1;
        self->s1 = self->origin_speed * self->T1 + 0.5 * self->a1 * self->T1 * self->T1;
        self->v1 = self->origin_speed + self->a1 * self->T1;

        /* init */
        self->T_count = 0;
        self->t = 0.0F;
        // self->out_cmd_pos = self->origin_pos;
        self->out_cmd_spd = 0.0F;
        self->out_cmd_acc = 0.0F;
    }
    if (self->running == 1)
    {
        self->t = self->T_count * self->Ts;

        if (self->t < self->t1)
        {
            self->tsec1 = self->t;

            self->out_cmd_pos = self->origin_pos +
                                self->origin_speed * self->tsec1 +
                                0.5 * self->a1 * self->tsec1 * self->tsec1;
            self->out_cmd_spd = self->origin_speed + self->a1 * self->tsec1;
            self->out_cmd_acc = self->a1;
        }
        else
        {
            /* recover */
            self->start = 0;
            self->running = 0;
            self->finished = 1;

            self->in_start = 0;
            self->running_pre = 0;
            self->T_count--;
        }

        self->T_count++;
    }
    if (self->start == 0 && self->running == 0)
    {
        self->t = self->T_count * self->Ts;
        self->tsec2 = self->t - self->t1;
        self->out_cmd_pos = self->origin_pos +
                            self->s1 +
                            self->v1 * self->tsec2;
        self->out_cmd_spd = self->v1;
        self->out_cmd_acc = 0.0F;

        self->T_count++;
    }

    /* 停止 */
    if (self->stop == 1)
    {
        self->stop = 0;
        self->start = 0;
        self->running = 0;
        self->finished = 0;

        self->in_start = 0;
        self->running_pre = 0;
        self->t = 0.0F;
        self->T_count = 0.0F;
        self->v1 = 0.0F;
        self->s1 = 0.0F;
        self->origin_pos = self->in_fdb_pos;

        self->out_cmd_pos = 0.0F;
        self->out_cmd_spd = 0.0F;
        self->out_cmd_acc = 0.0F;

        self->in_target_speed = 0.0F;
        self->target_speed_last = 0.0F;
        self->target_speed_old = 0.0F;
        self->target_speed = 0.0F;
    }

    return self->out_cmd_pos;
}

float TrapezoidCurve_Run_PosDoublePlan(struct TrapezoidCurve *self)
{
    if (self->param_auto_plan == TRUE)
    {
        if (abs_bm(self->in_target_pos - self->in_fdb_pos) > 1.0e-1F)
        {
            self->in_start = 1;
            self->target_pos_old = self->target_pos;
            self->target_pos = self->in_target_pos;
        }
        self->target_pos_last = self->in_target_pos;
    }

    /* 延迟启动 */
    if (self->in_start == 1 && self->running_pre == 0)
    {
        self->count = 0;
        self->running_pre = 1;

        self->origin_pos = self->in_fdb_pos;
        self->out_cmd_pos = self->origin_pos;
        self->out_cmd_spd = 0.0F;
        self->out_cmd_acc = 0.0F;
    }
    else if (self->running_pre == 1)
    {
        if (self->count < self->param_start_delay_count)
        {
            self->count++;
        }
        else
        {
            self->start = 1;
        }
    }

    /* 轨迹规划 */
    if (self->start == 1 && self->running == 0)
    {
        // self->start = 0;
        self->running = 1;

        /* plan */
        self->displacement = self->target_pos - self->origin_pos;
        self->dir = sign_bm(self->displacement);
        self->dir_speed = sign_bm(self->param_double_plan_max_speed_rad_s[1] - self->param_double_plan_max_speed_rad_s[0]);

        self->a1 = self->dir * self->param_double_plan_max_acc_rad_s2[0];
        self->a2 = 0.0F;
        self->a3 = self->dir * self->dir_speed * self->param_double_plan_max_acc_rad_s2[1];
        self->a4 = 0.0F;
        self->a5 = -self->dir * self->dir_speed * self->param_double_plan_max_acc_rad_s2[1];
        self->a6 = 0.0F;
        self->a7 = -self->dir * self->param_double_plan_max_acc_rad_s2[0];

        /* judge mode */
        self->Tk1 = self->param_double_plan_max_speed_rad_s[0] / self->param_double_plan_max_acc_rad_s2[0];
        self->sk1 = 0.5 * self->param_double_plan_max_acc_rad_s2[0] * self->Tk1 * self->Tk1;

        self->Tk2 = self->param_double_plan_1st_constant_speed_distance_rad / self->param_double_plan_max_speed_rad_s[0];
        self->sk2 = self->param_double_plan_1st_constant_speed_distance_rad;

        self->Tk3 = abs_bm(self->param_double_plan_max_speed_rad_s[1] - self->param_double_plan_max_speed_rad_s[0]) / self->param_double_plan_max_acc_rad_s2[1];
        self->sk3 = self->param_double_plan_max_speed_rad_s[0] * self->Tk3 + 0.5 * self->a3 * self->Tk3 * self->Tk3;

        if (abs_bm(self->displacement) < 2 * self->sk1)
        {
            self->traj_type = TrapezoidCurveTrajMode_DoublePlanTraj_Mode1;
        }
        else if (abs_bm(self->displacement) < 2 * (self->sk1 + self->sk2))
        {
            self->traj_type = TrapezoidCurveTrajMode_DoublePlanTraj_Mode2;
        }
        else if (abs_bm(self->displacement) < 2 * (self->sk1 + self->sk2 + self->sk3))
        {
            self->traj_type = TrapezoidCurveTrajMode_DoublePlanTraj_Mode3;
        }
        else
        {
            self->traj_type = TrapezoidCurveTrajMode_DoublePlanTraj_Mode4;
        }

        // self->traj_type = TrapezoidCurveTrajMode_DoublePlanTraj_Mode4;
        switch (self->traj_type)
        {
        case TrapezoidCurveTrajMode_DoublePlanTraj_Mode1:
            self->T1 = bm_sqrt_carmack(abs_bm(self->displacement) / self->param_double_plan_max_acc_rad_s2[0]);
            self->T2 = 0.0F;
            self->T3 = 0.0F;
            self->T5 = 0.0F;
            self->T6 = 0.0F;
            self->T7 = self->T1;

            self->v1 = self->a1 * self->T1;
            self->v2 = self->v1;
            self->v3 = self->v2 + self->a3 * self->T3;
            self->v4 = self->v3;
            self->v5 = self->v4 + self->a5 * self->T5;
            self->v6 = self->v5;
            self->v7 = self->v6 + self->a7 * self->T7;

            self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            self->s2 = 0.0F; // self->dir * self->sk2;
            self->s3 = 0.0F; // self->v2 * self->T3 + 0.5 * self->a3 * self->T3 * self->T3;
            self->s4 = 0.0F; // self->displacement - 2 * (self->s1 + self->s2 + self->s3);
            self->s5 = self->s3;
            self->s6 = self->s2;
            self->s7 = self->s1;

            self->T4 = 0.0F;
            break;
        case TrapezoidCurveTrajMode_DoublePlanTraj_Mode2:
            self->T1 = self->Tk1;

            self->v1 = self->a1 * self->T1;
            self->s1 = 0.5 * self->a1 * self->T1 * self->T1;

            self->T2 = (self->displacement / 2 - self->s1) / self->v1;
            self->T3 = 0.0F;
            self->T5 = 0.0F;
            self->T6 = self->T2;
            self->T7 = self->T1;

            // self->v1 = self->a1 * self->T1;
            self->v2 = self->v1;
            self->v3 = self->v2 + self->a3 * self->T3;
            self->v4 = self->v3;
            self->v5 = self->v4 + self->a5 * self->T5;
            self->v6 = self->v5;
            self->v7 = self->v6 + self->a7 * self->T7;

            // self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            self->s2 = self->v1 * self->T2;
            self->s3 = 0.0F; // self->v2 * self->T3 + 0.5 * self->a3 * self->T3 * self->T3;
            self->s4 = 0.0F; // self->displacement - 2 * (self->s1 + self->s2 + self->s3);
            self->s5 = self->s3;
            self->s6 = self->s2;
            self->s7 = self->s1;

            self->T4 = 0.0F;
            break;
        case TrapezoidCurveTrajMode_DoublePlanTraj_Mode3:
            self->T1 = self->Tk1;
            self->T2 = self->Tk2;

            self->v1 = self->a1 * self->T1;
            self->v2 = self->v1;
            self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            self->s2 = self->v1 * self->T2;

            float a, b, c;
            a = 0.5 * self->a3;
            b = self->v2;
            c = (2 * (self->s1 + self->s2) - self->displacement) / 2;
            self->T3 = (-b + bm_sqrt_carmack(b * b - 4 * a * c)) / (2 * a);
            self->T5 = self->T3;

            self->T6 = self->T2;
            self->T7 = self->T1;

            // self->v1 = self->a1 * self->T1;
            // self->v2 = self->v1;
            self->v3 = self->v2 + self->a3 * self->T3;
            self->v4 = self->v3;
            self->v5 = self->v4 + self->a5 * self->T5;
            self->v6 = self->v5;
            self->v7 = self->v6 + self->a7 * self->T7;

            // self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            // self->s2 = self->dir * self->sk2;
            self->s3 = self->v2 * self->T3 + 0.5 * self->a3 * self->T3 * self->T3;
            self->s4 = 0.0F; // self->displacement - 2 * (self->s1 + self->s2 + self->s3);
            self->s5 = self->s3;
            self->s6 = self->s2;
            self->s7 = self->s1;

            self->T4 = self->s4 / self->v3;
            break;
        case TrapezoidCurveTrajMode_DoublePlanTraj_Mode4:
            self->T1 = self->Tk1;
            self->T2 = self->Tk2;
            self->T3 = self->Tk3;
            self->T5 = self->T3;
            self->T6 = self->T2;
            self->T7 = self->T1;

            self->v1 = self->a1 * self->T1;
            self->v2 = self->v1;
            self->v3 = self->v2 + self->a3 * self->T3;
            self->v4 = self->v3;
            self->v5 = self->v4 + self->a5 * self->T5;
            self->v6 = self->v5;
            self->v7 = self->v6 + self->a7 * self->T7;

            self->s1 = 0.5 * self->a1 * self->T1 * self->T1;
            self->s2 = self->v1 * self->T2;
            self->s3 = self->v2 * self->T3 + 0.5 * self->a3 * self->T3 * self->T3;
            self->s4 = self->displacement - 2 * (self->s1 + self->s2 + self->s3);
            self->s5 = self->s3;
            self->s6 = self->s2;
            self->s7 = self->s1;

            self->T4 = self->s4 / self->v3;
            break;
        }

        self->t1 = self->T1;
        self->t2 = self->T1 + self->T2;
        self->t3 = self->T1 + self->T2 + self->T3;
        self->t4 = self->T1 + self->T2 + self->T3 + self->T4;
        self->t5 = self->T1 + self->T2 + self->T3 + self->T4 + self->T5;
        self->t6 = self->T1 + self->T2 + self->T3 + self->T4 + self->T5 + self->T6;
        self->t7 = self->T1 + self->T2 + self->T3 + self->T4 + self->T5 + self->T6 + self->T7;

        // DBprintfDMAAdv_Add("t:%s %s %s %s %s\r\n", float2str(self->t1, 4),
        //                    float2str(self->t2, 4),
        //                    float2str(self->t3, 4),
        //                    float2str(self->t4, 4),
        //                    float2str(self->t5, 4));

        /* init */
        self->T_count = 0;
        self->t = 0.0F;
        self->T_count_max = (uint32_t)(self->t7 / self->Ts);
    }
    else if (self->running == 1)
    {
        self->t = self->T_count * self->Ts;

        if (self->t < self->t1)
        {
            self->tsec1 = self->t;
            self->out_cmd_pos = self->origin_pos +
                                0.5 * self->a1 * self->tsec1 * self->tsec1;
            self->out_cmd_spd = self->a1 * self->tsec1;
            self->out_cmd_acc = self->a1;
        }
        // else if (self->t <= 3600.0F)
        else if (self->t < self->t2)
        {
            self->tsec2 = self->t - self->t1;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 +
                                self->v1 * self->tsec2;
            self->out_cmd_spd = self->v1;
            self->out_cmd_acc = self->a2;
        }
        else if (self->t < self->t3)
        {
            self->tsec3 = self->t - self->t2;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 + self->s2 +
                                self->v2 * self->tsec3 +
                                0.5 * self->a3 * self->tsec3 * self->tsec3;
            self->out_cmd_spd = self->v2 + self->a3 * self->tsec3;
            self->out_cmd_acc = self->a3;
        }
        else if (self->t < self->t4)
        {
            self->tsec4 = self->t - self->t3;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 + self->s2 + self->s3 +
                                self->v3 * self->tsec4;
            self->out_cmd_spd = self->v3;
            self->out_cmd_acc = self->a4;
        }
        else if (self->t < self->t5)
        {
            self->tsec5 = self->t - self->t4;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 + self->s2 + self->s3 + self->s4 +
                                self->v4 * self->tsec5 +
                                0.5 * self->a5 * self->tsec5 * self->tsec5;
            self->out_cmd_spd = self->v4 + self->a5 * self->tsec5;
            self->out_cmd_acc = self->a5;
        }
        else if (self->t < self->t6)
        {
            self->tsec6 = self->t - self->t5;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 + self->s2 + self->s3 + self->s4 + self->s5 +
                                self->v5 * self->tsec6;
            self->out_cmd_spd = self->v5;
            self->out_cmd_acc = self->a6;
        }
        else if (self->t < self->t7)
        {
            self->tsec7 = self->t - self->t6;
            self->out_cmd_pos = self->origin_pos +
                                self->s1 + self->s2 + self->s3 + self->s4 + self->s5 + self->s6 +
                                self->v6 * self->tsec7 +
                                0.5 * self->a7 * self->tsec7 * self->tsec7;
            self->out_cmd_spd = self->v6 + self->a7 * self->tsec7;
            self->out_cmd_acc = self->a7;
        }
        else
        {
            self->start = 0;
            self->running = 0;
            self->finished = 1;
            self->in_target_pos = self->in_fdb_pos;

            self->in_start = 0;
            self->running_pre = 0;
        }

        /* 停止 */
        if (self->stop == 1)
        {
            self->stop = 0;
            self->start = 0;
            self->running = 0;
            self->finished = 0;

            self->in_start = 0;
            self->running_pre = 0;

            self->origin_pos = self->in_fdb_pos;

            self->in_target_pos = self->in_fdb_pos;
            self->target_pos_last = 0.0F;
            self->target_pos_old = 0.0F;
            self->target_pos = 0.0F;
        }

        self->T_count++;
    }

    return self->out_cmd_pos;
}